/* 
* @Author: obsidian
* @Date:   2015-12-13 22:04:31
* @Last Modified by:   obsidian
* @Last Modified time: 2016-05-11 15:29:38
*/
#include "GPS.h"


#include "driver/delay/inc/delay.h"
#include "driver/math/inc/math.h"
#include "driver/led/inc/led.h"
#include "driver/sd/file.h"
#include "module/ledboard/ledboard.h"
#include "control/GPSpath/GPSpath.h"
#include "gps_driver.h"
#include "math.h"
#include "control/car.h"
#include "module/lcd/lcd_global.h"

#ifndef PI
#define PI 3.1415926
#endif

float my_distance ;
void GPS_update(void);
//void GPS_init(struct GPS *pgps);
void Gyro_Update(void);
void GPS_init(void);
void gps_save(void);
void gps_init(void);
void gps_read_sd(void);
void gps_clear(void);

static float lasr_dis1;
static float lasr_dis2;

//extern struct Ledboard ledboard;
struct GPS gps =
{

	.front_point        =   {0, 0},
	.rear_point         =   {0, 0},
	.start_point        =   {0, 0},
	.start_angle        =   0,       //start_angle
	.radian             =   0,        //radian
	.angle              =   0,     //angle
	.send_status        =   0,       //status
	.base_radian        =   0,       //base_radian
	.car_len            =   287.5,  //carlen
    .led_dis            =   0,
	.status             =   0,        //status
	.direction          =   0,     //direction
	.d_dis              =   0,     //d_dis
 	.speed              =   0,     //speed


	.init               =   gps_init,
	.gyro_update        =   Gyro_Update,
	.GPS_init           =   GPS_init,
	.GPS_update         =   GPS_update,
	.save               =   gps_save,
	.read_from_sd       =   gps_read_sd,
	.clear              =   gps_clear,
};

struct Encoder_property encoder_property[ENCODER_NUM] = 
{
	//count_per_rotation        TIMx     direction              convert
	{ .count_per_rotation = 2048, .TIMx = TIM3, .direction = -1,    .convert = 0.1026483268},
	// {    2048,                   0,              TIM2,       -1,         0.1026483268},
	{   2048,                   0,              TIM8,       1,          0.1037775010}
	// {   2048,                    0,              TIM4,       1,          0.1037775010}

};


struct Gyro gyro = 
{
	1.60011,        //gyro_convert1 //1.60011
	1.60322,        //gyro_convert2 //1.60322
	{
		0,      //value
		0       //count
	},
	{
		0,      //gyro_float
		0,      //float_state
		0,      //gyro_float_flag1
		0,      //gyro_float_flag2
		0,      //gyro_float_flag
		0,      //gyro_error_flag
	},
	0,          //gyro_total
	0,          //gyro_total_nofloat
	0,          //gyro_radian_total
	0,          //gyro_angle_total
	0,          //gyro_float_total1
	0           //gyro_float_total2
};
struct Encoder_data encoder[ENCODER_NUM] = 
{
	{
		0,  //distance
		0,  //delta_distance
		0,  //num_of_rotation
		0,  //last_count
		0,  //total_count   
	},
	{
		0,  //distance
		0,  //delta_distance
		0,  //num_of_rotation
		0,  //last_count
		0,  //total_count   
	},
};


static  struct Encoder_data last_encoder[ENCODER_NUM] = 
{
	{0, 0,  0, 0 ,0},{0,    0,  0, 0,0,}, //last_encoder
};


static void Encoder_Update(void);
static void Gyro_Clear(void);
static void UART1_DMA_EN(void);
static void Gyro_CheckFloat(void);


void gps_clear()
{
	char i = 0;
	Gyro_Clear();
	gps.rear_point.x = gps.start_point.x;
	gps.rear_point.y = gps.start_point.y;
	gps.base_radian = gps.start_angle * PI / 180.0;
	gps.front_point.x = gps.rear_point.x + gps.car_len * sin(gps.base_radian);
	gps.front_point.y = gps.rear_point.y + gps.car_len * cos(gps.base_radian);
	gps.radian = gps.base_radian;
	gps.angle = gps.radian * 180 /PI;
	for(i = 0;i < 2;i ++)
	{
		encoder[i].num_of_rotation = 0;
		encoder_property[i].TIMx->CNT = 0;
		encoder[i].last_count = 0;
		encoder[i].total_count = 0;
		encoder[i].distance = 0;   
		last_encoder[i].distance = 0;   
	}
		lasr_dis1 = 0;
		lasr_dis2 = 0;
}

void GPS_init()
{
	Encoder_Timer_Config();
	//UART1_DMA_init();
	//UART1_DMA_EN();
	//Gyro_CheckFloat();
}

static struct GPS dup_gps;
static struct Gyro dup_gyro;
static struct Encoder_property dup_encoder[2];

void gps_init()
{
	GPS_init();
}
void gps_read_sd()
{
		
	unsigned int k = 0;
	car.strjoint("GPS");
	f_open(&fil,car.path,FA_READ);
	if(fil.fs != 0)
	{
		f_read(&fil, (char *)&dup_gps, sizeof(struct GPS), &k);
		gps.start_point.x = dup_gps.start_point.x;
		gps.start_point.y = dup_gps.start_point.y;
		gps.start_angle = dup_gps.start_angle;
		file_close();
	}
	car.strjoint("encoder");
	f_open(&fil,car.path,FA_READ);
	if(fil.fs != 0)
	{
		f_read(&fil, (char *)dup_encoder, 2 * sizeof(struct Encoder_property), &k);
		encoder_property[0].convert = dup_encoder[0].convert;
		encoder_property[1].convert = dup_encoder[1].convert;
		file_close();
	}
	car.strjoint("gyro");
	f_open(&fil,car.path,FA_READ);
	if(fil.fs != 0)
	{
		f_read(&fil, (char *)&dup_gyro, sizeof(struct Gyro), &k);
		gyro.gyro_convert1 = dup_gyro.gyro_convert1;
		gyro.gyro_convert2 = dup_gyro.gyro_convert2;
		file_close();
	}
}
void gps_save()
{
	unsigned int k = 0;
	car.strjoint("GPS");
	f_open(&fil,car.path,FA_WRITE | FA_CREATE_ALWAYS);
	if(fil.fs == 0){return;}
	f_write(&fil, (char *)&gps, sizeof(struct GPS), &k);
	file_close();
	car.strjoint("encoder");
	f_open(&fil,car.path,FA_WRITE | FA_CREATE_ALWAYS);  if(fil.fs == 0){return;}
	f_write(&fil, (char *)&encoder_property, 2 * sizeof(struct Encoder_property), &k);
	file_close();
	car.strjoint("gyro");
	f_open(&fil,car.path,FA_WRITE | FA_CREATE_ALWAYS);
	if(fil.fs == 0){return;}
	f_write(&fil, (char *)&gyro, sizeof(struct Gyro), &k);
	file_close();
}

static float lasr_dis1 = 0;
static float lasr_dis2 = 0;
static float last_encoder_dis = 0;

float gps_update_last_radian = 0;
void GPS_update()
{
	Encoder_Update();
	double  delta_x                 = 0;
	double delta_y                  = 0;
	float delta_distance1           = 0;
	float delta_distance2           = 0;
// 	static  float last_radian       = 0;
	float radian                    = 0;
	float l                         = 0;
	static char speedcount          = 0;
	float       speed_del = 0;
	float       encoder_dis         = 0;

	if(gps.status == OFF)
	{
		return;
	}
	//radian
	gps.radian = gyro.gyro_radian_total  +gps.base_radian;
	gps.angle  = gyro.gyro_angle_total + gps.base_radian * 180 / PI;

	encoder[0].delta_distance = encoder[0].distance - last_encoder[0].distance;
	encoder[1].delta_distance = encoder[1].distance - last_encoder[1].distance;
	delta_distance1 = encoder[0].delta_distance;
	delta_distance2 = encoder[1].delta_distance;
	
	//speed
	speedcount ++;
	if(speedcount >= 10)
	{
		speed_del = (encoder[0].distance - lasr_dis1 + encoder[1].distance - lasr_dis2);
		speed_del = speed_del / 2;
		gps.speed = speed_del * 40;//gps.speed = (speed_del / (5 *10))*1000  (mm/s);
		lasr_dis1 = encoder[0].distance;
		lasr_dis2 = encoder[1].distance;
		
		speedcount  = 0;
	}

	// encoder_dis = (encoder[0].distance + encoder[1].distance) / 2;
	// if(encoder_dis - last_encoder_dis > 5)
	// {
	// 	gpspath.ctl_flag = 1;
 //        last_encoder_dis = encoder_dis;

	// }

	if(fabs(delta_distance1) < 1 && fabs(delta_distance2) < 1)
		{return;}
	//gps
	radian = (gps.radian + gps_update_last_radian) / 2;
	if(radian>PI)
		radian-=2*PI;
	if(radian<-PI)
		radian+=2*PI;
	l = ( delta_distance1 + delta_distance2 ) / 2;
	delta_x = l * sin(radian);
	delta_y = l * cos(radian);

	gps.rear_point.y += delta_y;
	gps.rear_point.x += delta_x;
	gps.front_point.x = gps.rear_point.x + gps.car_len * sin(radian);
	gps.front_point.y = gps.rear_point.y + gps.car_len * cos(radian);
	
	gps.send_status = 1;

	last_encoder[0].distance = encoder[0].distance;
	last_encoder[1].distance = encoder[1].distance;
	gps_update_last_radian = gps.radian;
}

signed int get_encoder(int i)
{
	return (int32_t)((int32_t)(encoder[i].num_of_rotation)*encoder_property[i].count_per_rotation +(int32_t)(encoder_property[i].TIMx->CNT));
}

signed int step_test[2];
void Encoder_Update()
{
	int i = 0;
	signed int encoder_count = 0;
	signed int step;
	for(i = 0; i < 2; i ++)
	{
		encoder_count = get_encoder(i);
		step = encoder_count - encoder[i].last_count;
		
		step_test[i] = step;
		
		if(step < 2000 && step > -2000)
		{
// 			encoder[i].last_count = encoder_count;
// 			encoder[i].total_count += step * encoder_property[i].direction;
// 			encoder[i].distance += step * encoder_property[i].convert * encoder_property[i].direction;

			encoder[i].last_count = encoder_count;
			encoder[i].total_count += step * encoder_property[i].direction;
			///if(step_test[0] < 3*step_test[1] && step_test[1] < 3*step_test[0])//不发生一个轮子空转的情况
			//{
				encoder[i].distance += step * encoder_property[i].convert * encoder_property[i].direction;
	    	my_distance = encoder[i].distance*(0.10);
			//}
		}
		else
		{
			encoder[i].last_count = encoder_count;}
		}
}

void Gyro_Update(void)
{

	struct Gyro*    pgyro = &gyro;
	pgyro->gyro_total+=pgyro->gyro_value.value;//1152 
	pgyro->gyro_total_nofloat = pgyro->gyro_total - pgyro->gyro_calibration.gyro_float*pgyro->gyro_value.count;
	pgyro->gyro_angle_total = pgyro->gyro_total_nofloat*0.0000001*pgyro->gyro_convert1;
	pgyro->gyro_angle_total = pgyro->gyro_angle_total - (int)(pgyro->gyro_angle_total/360)*360;//强制类型转换直接舍去小数点
	
	if(pgyro->gyro_angle_total > 180.0)
		pgyro->gyro_angle_total -= 360.0;
	if(pgyro->gyro_angle_total < -180.0)
		pgyro->gyro_angle_total += 360.0;
	
	pgyro->gyro_radian_total = pgyro->gyro_angle_total* 3.1415926 / 180;
}

void Gyro_CheckFloat(void)
{
	struct Gyro*    pgyro = &gyro;
	struct Gyro_calibration* pgyro_calibration = &gyro.gyro_calibration;
	

	LED2_on;
	Gyro_Clear();
	pgyro_calibration->gyro_float_flag=1;
	pgyro_calibration->gyro_float_flag1=1;
	pgyro_calibration->gyro_float_flag2=1;
	while(pgyro_calibration->gyro_float_flag)
	{
		if(pgyro->gyro_float_total2 - pgyro->gyro_float_total1  ==0)
		{
			pgyro_calibration->float_state = CF_ERROR;
		}
		else 
		{
			pgyro_calibration->gyro_float = (pgyro->gyro_float_total2 -pgyro->gyro_float_total1  )/10240;//*Gyro_Float每一次的漂移量
			pgyro_calibration->float_state = CF_CHANGED;
		}

	}
	Gyro_Clear();
	if(pgyro_calibration->float_state == CF_ERROR)
	{
		while(1)
		{
			delay_ms(800);
			LED2_off;
			delay_ms(800);
			LED2_on;
		}
	}
	else
	{
	}
	LED2_off;
}
void Gyro_Clear()
{
	gyro.gyro_value.count = 0;
	gyro.gyro_value.value = 0;
	gyro.gyro_total = 0;
	gyro.gyro_total_nofloat = 0;
}


//不知什么原因  陀螺仪传回来的数据只能用
//if(USART_GetFlagStatus(USART1, USART_FLAG_ORE) != RESET)
//这个判断接收到   用DMA通道也可以
int gyrocount = 0;
void UART1_DMA_EN(void)
{
	LED2_on;
	uint8_t gyro_start,j;
	uint8_t gyro_temp;
	gyro_temp = 0;
	gyro_start=1; 
	j=0;
	
	while(gyro_start)
	{
		if(j==0)
		{
			if(USART_GetFlagStatus(USART1, USART_FLAG_ORE) != RESET)
			{
				USART_ClearFlag(USART1, USART_FLAG_ORE);
				gyro_temp = USART_ReceiveData(USART1);
			}

			if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) 
			{
				USART_ClearITPendingBit(USART1, USART_IT_RXNE);
				gyro_temp = USART_ReceiveData(USART1);
			}
			
			if(gyro_temp == 0xDD)
			{
				j=1;
			}
		}
		else
		{
			if(USART_GetFlagStatus(USART1, USART_FLAG_ORE) != RESET)
			{
				USART_ClearFlag(USART1, USART_FLAG_ORE);
				gyro_temp = USART_ReceiveData(USART1);
				j++;
			}

			if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) 
			{
				USART_ClearITPendingBit(USART1, USART_IT_RXNE);
				gyro_temp = USART_ReceiveData(USART1);
				j++;
			}

			if(j==9)
			{
				if(gyro_temp==0xDD)
				{
					gyro_start = 0;     
					DMA_Cmd(DMA1_Channel5, ENABLE);
				}
				else
				{
					j=0;
				}
			}else if(j>9)
			{
				j=0;
			}
		}
	}
	delay_ms(300);
	LED2_off;
}


//陀螺仪从usart发回来的数据一帧4个字节  每帧开始(也许是结尾)以0xDD作为标记 
//DMA的缓冲区大小是8个字节 所以其实每发回来两次数据只使用了一次   不知道这样做的原因是什么 
//现在陀螺仪的误差大约是转一圈在1度以内  今年这个误差可以接受  如果以后需要提高陀螺仪的精度
//可以考虑试一试将数据全部用上(能提高精度的可能性不大  因为数据更新的速度感觉已经足够快)--2016.4.9
void DMA1_Channel5_IRQHandler(void)
{
	if(DMA_GetITStatus(DMA1_IT_GL5)==SET)
	{
		if (USART1_RxBuffer[0] == 20) restart3();
		else ledboard.offset=USART1_RxBuffer[0];
		
		DMA_ClearITPendingBit(DMA1_IT_GL5);
	}
}

/*
Timer
 */

// void TIM3_IRQHandler(void)
// {
// 	TIM_ClearITPendingBit(TIM3,TIM_IT_Update);
// 	if(!(TIM3->CR1&0x0010))
// 	{/* count up overflow interrupt */
// 		encoder[0].num_of_rotation += 25;
// 	}
// 	else
// 	{/* count down overflow interrupt */
// 		encoder[0].num_of_rotation -= 25;
// 	}
// }


// void TIM4_IRQHandler(void)
// {
//  TIM_ClearITPendingBit(TIM3,TIM_IT_Update);
//     if(!(TIM3->CR1&0x0010))
//     {/* count up overflow interrupt */
//         encoder[0].num_of_rotation++;
//     }
//     else
//     {/* count down overflow interrupt */
//         encoder[0].num_of_rotation--;
//     }
// }


// void TIM2_IRQHandler(void)
// {
//  TIM_ClearITPendingBit(TIM2,TIM_IT_Update);
//     if(!(TIM2->CR1&0x0010))
//     {/* count up overflow interrupt */
//         encoder[1].num_of_rotation++;
//     }
//     else
//     {/* count down overflow interrupt */
//         encoder[1].num_of_rotation--;
//     }
// }

void TIM8_UP_IRQHandler(void)
{
	TIM_ClearITPendingBit(TIM8,TIM_IT_Update);
	if(!(TIM8->CR1&0x0010))
	{/* count up overflow interrupt */
		encoder[1].num_of_rotation += 25;
	}
	else
	{/* count down overflow interrupt */
		encoder[1].num_of_rotation -= 25;
	}
}

